#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include "nav_msg_pkg/msg/waypoint.hpp"

class PoseSubscriberPublisher : public rclcpp::Node
{
public:
    PoseSubscriberPublisher() : Node("test_node")
    {
        // 创建订阅者
        subscription_ = this->create_subscription<nav_msg_pkg::msg::Waypoint>(
            "/nav_server/nav_point", 10, std::bind(&PoseSubscriberPublisher::nav_point_callback, this, std::placeholders::_1));
        RCLCPP_INFO(this->get_logger(), "test_node is running");

    }

private:
    rclcpp::Subscription<nav_msg_pkg::msg::Waypoint>::SharedPtr subscription_;

    void nav_point_callback(const nav_msg_pkg::msg::Waypoint::SharedPtr msg)
    {
        // RCLCPP_INFO(this->get_logger(), "收到航点信息");
        RCLCPP_INFO(this->get_logger(), "接收任务,目标航点 name=%s,type=%s,x=%f,y=%f",
                    msg->name.c_str(),
                    msg->type.c_str(),
                    msg->pose.position.x,
                    msg->pose.position.y);
        // 处理接收到的 Pose 消息
    }
   
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PoseSubscriberPublisher>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}